#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <array>

#include <Eigen/Dense>

// 输入：一个四元数（实部和虚部）
// 输出：一个3x3的旋转矩阵
Eigen::Matrix3d quaternionToRotationMatrix(double w, double x, double y, double z) {
    Eigen::Matrix3d rotationMatrix;

    double sqw = w * w;
    double sqx = x * x;
    double sqy = y * y;
    double sqz = z * z;

    // 旋转矩阵的第一列
    rotationMatrix(0, 0) = sqx - sqy - sqz + sqw;
    rotationMatrix(1, 0) = 2.0 * (x * y + z * w);
    rotationMatrix(2, 0) = 2.0 * (x * z - y * w);

    // 旋转矩阵的第二列
    rotationMatrix(0, 1) = 2.0 * (x * y - z * w);
    rotationMatrix(1, 1) = -sqx + sqy - sqz + sqw;
    rotationMatrix(2, 1) = 2.0 * (y * z + x * w);

    // 旋转矩阵的第三列
    rotationMatrix(0, 2) = 2.0 * (x * z + y * w);
    rotationMatrix(1, 2) = 2.0 * (y * z - x * w);
    rotationMatrix(2, 2) = -sqx - sqy + sqz + sqw;

    return rotationMatrix;
}


class Quaternion {
public:
    float w, x, y, z;
 
    Quaternion(float w, float x, float y, float z) : w(w), x(x), y(y), z(z) {}
 
    std::array<std::array<float, 3>, 3> toRotationMatrix() const {
        std::array<std::array<float, 3>, 3> matrix;
        float xx = x * x;
        float yy = y * y;
        float zz = z * z;
        float xy = x * y;
        float xz = x * z;
        float yz = y * z;
        float wx = w * x;
        float wy = w * y;
        float wz = w * z;
 
        matrix[0][0] = 1 - 2 * (yy + zz);
        matrix[0][1] = 2 * (xy - wz);
        matrix[0][2] = 2 * (xz + wy);
        matrix[1][0] = 2 * (xy + wz);
        matrix[1][1] = 1 - 2 * (xx + zz);
        matrix[1][2] = 2 * (yz - wx);
        matrix[2][0] = 2 * (xz - wy);
        matrix[2][1] = 2 * (yz + wx);
        matrix[2][2] = 1 - 2 * (xx + yy);
 
        return matrix;
    }
};
 
int main() {
    Quaternion q(0.707107f,0.0f,0.707107f, 0.0f); // Identity quaternion for example
    auto matrix = q.toRotationMatrix();
    std::cout << "Rotation Matrix:\n";
    for (const auto& row : matrix) {
        for (float val : row) {
            std::cout << val << " ";
        }
        std::cout << "\n";
    }

    auto m1 = quaternionToRotationMatrix(0.0f, 0.707107f,0.0f,0.707107f);
    std::cout << "Rotation Matrix:\n";
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            std::cout << m1(i, j) << " ";
        }
        std::cout << "\n";
    }
    return 0;
}